/*
 * CameraSpace.h
 *
 *  Created on: Mar 22, 2013
 *      Author: ohnozzy
 */

#ifndef CAMERASPACE_H_
#define CAMERASPACE_H_
#include <ompl/base/StateSpace.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/spaces/SO3StateSpace.h>

namespace flyingcamera
{
    namespace base
    {

        /** \brief A state space representing SE(3) */
        class CameraSpace : public ompl::base::CompoundStateSpace
        {
        public:

            /** \brief A state in SE(3): position = (x, y, z), quaternion = (x, y, z, w) */
            class StateType : public ompl::base::CompoundStateSpace::StateType
            {
            public:
                StateType(void) : ompl::base::CompoundStateSpace::StateType()
                {
                }

                /** \brief Get the X component of the state */
                double getT(void) const
                {
                    return as<ompl::base::RealVectorStateSpace::StateType>(0)->values[0];
                }
                 const ompl::base::SO3StateSpace::StateType& rotation(void) const
                {
                         return *as<ompl::base::SO3StateSpace::StateType>(1);
                 }


                /** \brief Get the rotation component of the state and allow changing it as well */
                 ompl::base::SO3StateSpace::StateType& rotation(void)
                {

                    return *as<ompl::base::SO3StateSpace::StateType>(1);
                }

                /** \brief Set the X component of the state */
                void setT(double t)
                {
                    as<ompl::base::RealVectorStateSpace::StateType>(0)->values[0] = t;
                }

            };

            CameraSpace(void) : ompl::base::CompoundStateSpace()
            {
                setName("Camera" + getName());
                addSubspace(ompl::base::StateSpacePtr(new ompl::base::RealVectorStateSpace(1)), 1.0);
                addSubspace(ompl::base::StateSpacePtr(new ompl::base::SO3StateSpace()), 1.0);
                ompl::base::RealVectorBounds bound(1);
                bound.setLow(0);
                bound.setHigh(1);
                as<ompl::base::RealVectorStateSpace>(0)->setBounds(bound);
                lock();
            }

            virtual ~CameraSpace(void)
            {
            }

            /** \copydoc RealVectorStateSpace::setBounds() */
            void setBounds(const ompl::base::RealVectorBounds &bounds)
            {
                as<ompl::base::RealVectorStateSpace>(0)->setBounds(bounds);
            }

            /** \copydoc RealVectorStateSpace::getBounds() */
            const ompl::base::RealVectorBounds& getBounds(void) const
            {
                return as<ompl::base::RealVectorStateSpace>(0)->getBounds();
            }

            virtual ompl::base::State* allocState(void) const;



        };
    }
}




#endif /* CAMERASPACE_H_ */
